import wx
import os
import serial
from time import clock
import time
#from rpy import *
from VideoCapture import Device
from opencv import  *
from PIL import Image
from ctypes import *
from opencv.cxcore import *

#windowSize = wx.Size(800,600)
windowSize = wx.Size(1350,700)
treePaneWidth = 200 
minTreePaneWidth = 100
itemListHeight = 300
minItemPaneHeight = 200

ID_SHOWIMAGE_BUTTON = 1001
ID_ENDSHOW_BUTTON = 1002
ID_CALIBRATE_BUTTON = 1003
ID_MOVELEFT_BUTTON = 1004
ID_MOVERIGHT_BUTTON = 1005
ID_TOGGLELASER_BUTTON = 1006
ID_SCAN_BUTTON = 1007
ID_SCANANGLE = 1008
ID_PORT_BUTTON = 1009
ID_CALIBRATE2_BUTTON = 1010
ID_TEST_BUTTON = 1011
ID_TEST2_BUTTON = 1012
ID_CLEAR_BUTTON = 1012
ID_CAM1_COMBO = 1021
ID_CAM2_COMBO = 1022
ID_SERIAL_COMBO = 1023
ID_CHK_USE_CAMERA1 = 1024
ID_CHK_USE_CAMERA2 = 1025

    
ID_FROM_FILE_BUTTON = 1031
ID_FIND_CHESSBOARD_BUTTON = 1032
ID_CALIBRATE_CAMERA_BUTTON = 1033
ID_STEREO_CALIBRATE_BUTTON = 1034
ID_STEREO_RECTIFY_BUTTON = 1035
ID_RECTIFY_MAP_BUTTON = 1036
ID_CAMTEST_COMBO= 1037
ID_CHK_REMAP = 1038

CHESSBOARD_WIDTH = 7
CHESSBOARD_HEIGHT = 6
N_CHESSBOARD = 20
IMAGE_WIDTH = 640
IMAGE_HEIGHT = 480
cvIMAGE_SIZE = cvSize( IMAGE_WIDTH, IMAGE_HEIGHT )


class CamView( wx.Window ):
  def __init__( self, parent, id, bitmap=None,size=(IMAGE_WIDTH,IMAGE_HEIGHT) ):
    wx.Window.__init__(self, parent,size=size )
    self.has_bitmap = False
    self.Bind( wx.EVT_PAINT, self.OnPaint )
    self.Bind( wx.EVT_SIZE, self.OnSize)
    self.Bind( wx.EVT_ERASE_BACKGROUND, self.OnEraseBackground)
    self.OnSize(None)
    #self.Set
    #if bitmap != None:
    #  self.SetBitmap( bitmap )

  def OnEraseBackground(self,evt):
    pass
  
  def SetBitmap( self, bitmap ):
    #print "set bitmap ", bitmap.GetWidth(), "x", bitmap.GetHeight()
    #print "set bitmap begins", clock()
    self.has_bitmap = True
    self.bitmap = bitmap
    self.UpdateBitmap()
    self.Refresh()
    #print "set bitmap ends", clock()

  def OnSize(self,evt):
    self.buffer = wx.EmptyBitmap( IMAGE_WIDTH,IMAGE_HEIGHT)
    #self.UpdateBitmap()

  def UpdateBitmap(self):
    dc = wx.BufferedDC( wx.ClientDC(self), self.buffer )
    dc.DrawBitmap( self.bitmap,0,0,True )
    
  
  def OnPaint(self,evt):
    #print "onpaint has_bitmap", self.has_bitmap
    bdc = wx.BufferedPaintDC( self, self.buffer )
    #brush = wx.Brush( "gray" )
    #dc.SetBackground( brush )
    #dc.Clear()
    #if self.has_bitmap:

class DoonoonCam:
  camera = None
  success_count = 0
  rectmapx = None
  rectmapy = None
  mapx = None
  mapy = None
  rectification_finished = True
  save_count = 0
  remap = False
  active = False
  def __init__(self, index=None, width=IMAGE_WIDTH,height=IMAGE_HEIGHT):
    #print "[",index,"]"
    self.width = width
    self.height = height
    if index != None:
      self.set_camera( index )
      
  def set_resolution( self, width = IMAGE_WIDTH, height = IMAGE_HEIGHT ):
    self.camera.setResolution( width, height )

  def set_camera( self, index ):
    try:
      self.camera = Device( devnum = index )
    except ValueError, e:
      print e
    self.index = index
    self.set_resolution( self.width, self.height )
    self.prepare_calibrate( CHESSBOARD_WIDTH, CHESSBOARD_HEIGHT, N_CHESSBOARD )

  def prepare_calibrate(self, board_w, board_h, n_boards ):
    self.board_w = board_w
    self.board_h = board_h
    self.board_n = board_w * board_h
    self.n_boards = n_boards
    self.intrinsic_matrix  = cvCreateMat( 3, 3, CV_32FC1 )
    self.distortion_coeffs = cvCreateMat( 4, 1, CV_32FC1 )
    self.point_counts      = cvCreateMat( self.n_boards, 1, CV_32SC1 )
    self.image_points      = cvCreateMat( self.n_boards * self.board_n, 2, CV_32FC1 )
    self.object_points     = cvCreateMat( self.n_boards * self.board_n, 3, CV_32FC1 )
    self.success_count = 0
    self.calibration_finished = False
    self.rectification_finished = False
    self.is_finding_chessboard = False

  def begin_find_chessboard(self):
    self.is_finding_chessboard = True

  def end_find_chessboard(self):
    self.is_finding_chessboard = False

  def find_chessboard(self ):
    # convert grabbed img to opencv bitmap 
    # before that, just save and load
    img = pil_to_ipl( self.img )
    #filename = "cam" + str( self.index ) + "_" + str( self.success_count ) + ".jpg"
    #self.img.save(  filename )
    #img = cvLoadImage( filename)

    gray_img = cvCreateImage( cvGetSize( img ), 8, 1 )
    sz = cvSize( self.board_w, self.board_h )
    ( found, corners ) = cvFindChessboardCorners( img, sz, None, None, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS )

    cvCvtColor( img, gray_img, CV_BGR2GRAY )
    corners = cvFindCornerSubPix( gray_img, corners, cvSize(11,11),cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1))

    cvDrawChessboardCorners( img, sz, corners, found )
    self.corner_img = ipl_to_pil( img )

    #cvNamedWindow( "Calibration" + str( self.index ) )
    #cvShowImage( "Calibration" + str(self.index), img )
    #print "len corners:", len(corners)
    #print "board_n:", self.board_n
    if len( corners ) == self.board_n :
      step = self.success_count * self.board_n
      i = step
      for j in range( self.board_n ):
        #print i, j
        self.image_points[i,0] = corners[j].x
        self.image_points[i,1] = corners[j].y
        self.object_points[i,0] = j / self.board_w
        self.object_points[i,1] = j % self.board_w
        self.object_points[i,2] = 0.0
        i+= 1
        j+= 1
      self.point_counts[self.success_count,0] = self.board_n
      return 1
    else:
      return 0

  def clear_image(self):
    self.img = None
    
  def grab_image(self):
    #print "grab_image", self.index
    self.img = self.get_image()

  def save_image(self,filename):
    self.img.save( filename )

  def load_image(self,filename):
    self.img = Image.open( filename )

  def get_image(self):
    #print "get_image", self.index
    pil_img = self.camera.getImage()
    #print "get image done", self.index
    #if self.is_finding_chessboard:
    self.img = pil_img
    #if self.rectification_finished:
    #  return self.get_rectified_image( pil_img )
    #if self.calibration_finished:
    #  return self.get_calibrated_image( pil_img )
    return pil_img

  def get_processed_image(self):
    if self.is_finding_chessboard:
      self.find_chessboard()
      return self.corner_img
    #return ipl_to_pil( pil_to_ipl( self.img ) )
    if not self.remap:
      return self.img
    if self.rectification_finished:
      return self.get_rectified_image()
    if self.calibration_finished:
      return self.get_calibrated_image()
    return self.img

  def get_calibrated_image(self):
    ipl_img = pil_to_ipl( self.img ) 
    t = cvCloneImage( ipl_img )
    cvRemap( t, ipl_img, self.mapx, self.mapy )
    return ipl_to_pil( ipl_img )

  def get_rectified_image(self ):
    #print pil_img.size[0], pil_img.size[1]
    if self.rectmapx == None:
      print "no rec map"
      return self.img
    #print "has rec map"
    #print self.img.size[0], self.img.size[1]
    #self.save_count += 1
    #filename = "images/rect_" + str( self.index ) + "_" + str( self.save_count ) + ".jpg"
    #self.save_image( filename )
    ipl_img = pil_to_ipl( self.img ) 
    #ipl_img = cvLoadImage( filename )
    #t = cvLoadImage( filename )
    #ipl_img = cvCreateImageHeader(cvSize(self.img.size[0], self.img.size[1]), IPL_DEPTH_8U, 3)
    #data = self.img.tostring('raw', 'RGB', self.img.size[0] * 3)
    #cvSetData(ipl_img, cast(data, POINTER(c_byte)), self.img.size[0] * 3)
    #cvCvtColor(ipl_img, ipl_img, CV_RGB2BGR)
    #ipl_img._depends = (data,)
    t = cvCloneImage( ipl_img )
    cvRemap( t, ipl_img, self.rectmapx, self.rectmapy )
    #filename = "images/remap_" + str( self.index ) + "_" + str( self.save_count ) + ".jpg"
    #cvSaveImage( filename, ipl_img )
    return ipl_to_pil( ipl_img )

  def calibrate(self, intrinsic = None, distortion = None):
    # print "calibrate ", self.index
    
    if intrinsic == None and distortion == None:
      #img = self.img
      cvSetIdentity( self.intrinsic_matrix )
      cvZero( self.distortion_coeffs )
      #cvSave( "matrices/object_points_" + str( self.index ) + ".xml", byref( self.object_points ) )
      #cvSave( "matrices/image_points_" + str( self.index ) + ".xml", byref( self.image_points ) )
      #cvSave( "matrices/points_counts_" + str( self.index ) + ".xml", byref( self.point_counts ) )
      
      if cvVersion == 100:
        #print "version 100"
        self.distortion_coeffs = cvCreateMat( 4, 1, CV_32FC1 )
      ( self.intrinsic_matrix, self.distortion_coeffs ) = cvCalibrateCamera2( self.object_points, self.image_points, self.point_counts, cvIMAGE_SIZE, None, self.distortion_coeffs, None, None, 0 )
      #print cal_result
      #print intrinsic
      #print distortion
      #self.calibration_finished = True
      #cvSave( "matrices/Intrinsics_" + str( self.index ) + ".xml", byref( intrinsic ) )
      #cvSave( "matrices/Distortion_" + str( self.index ) + ".xml", byref( distortion ) )
      #intrinsic = cvLoad( "Intrinsics_" + str( self.index ) + ".xml" )
      #distortion = cvLoad( "Distortion_" + str( self.index ) + ".xml" )
    self.mapx = cvCreateImage( cvIMAGE_SIZE, IPL_DEPTH_32F, 1 )
    self.mapy = cvCreateImage( cvIMAGE_SIZE, IPL_DEPTH_32F, 1 )
    #cvInitUndistortMap( intrinsic, distortion, self.mapx, self.mapy )
    cvInitUndistortMap( self.intrinsic_matrix, self.distortion_coeffs, self.mapx, self.mapy )
    #cvSave( "matrices/mapx_" + str( self.index ) + ".xml", byref( self.mapx ) )
    #cvSave( "matrices/mapy_" + str( self.index ) + ".xml", byref( self.mapy ) )
    
    self.calibration_finished = True
    #for i in range( 3 ):
    #  print intrinsic[i,0], intrinsic[i,1], intrinsic[i,2]
    #for i in range( 4 ):
    #  print distortion[i,0]
    
  def load_intrinsics(self):
    self.intrinsic_matrix = self.load_matrix( "intrinsics" )

  def load_distcoeffs(self):
    self.distortion_coeffs = self.load_matrix( "distcoeffs" )

  def load_maps(self):
    self.mapx = self.load_matrix( "mapx" )
    self.mapy = self.load_matrix( "mapy" )

  def load_rectmaps(self):
    self.rectmapx = self.load_matrix( "rectmapx" )
    self.rectmapy = self.load_matrix( "rectmapy" )

  def load_matrix( self, matrix_name ):
    matrix = cvLoadCast( self.get_matrix_name( matrix_name ), CvMat)
    return matrix

  def load_point_counts(self):
    self.point_counts = self.load_matrix( "point_counts" )

  def load_image_points(self):
    self.image_points = self.load_matrix( "image_points" )

  def load_object_points(self):
    self.object_points = self.load_matrix( "object_points" )


  def get_matrix_name( self, matrix_name ):
    return "matrices/camera" + str( self.index ) + "_" + matrix_name + ".xml"  
  
  def save_matrix( self, matrix_name, matrix ):
    return cvSave( self.get_matrix_name( matrix_name ) , byref( matrix ) )

  def save_intrinsics(self):
    return self.save_matrix( "intrinsics", self.intrinsic_matrix )

  def save_distcoeffs(self):
    return self.save_matrix( "distcoeffs", self.distortion_coeffs )

  def save_object_points(self):
    return self.save_matrix( "object_points", self.object_points )

  def save_image_points(self):
    return self.save_matrix( "image_points", self.image_points )

  def save_point_counts(self):
    return self.save_matrix( "point_counts", self.point_counts )

  def save_maps(self):
    self.save_matrix( "mapx", self.mapx )
    self.save_matrix( "mapy", self.mapy )

  def save_rectmaps(self):
    self.save_matrix( "rectmapx", self.rectmapx )
    self.save_matrix( "rectmapy", self.rectmapy )

  def activate(self):
    self.active = True
    #print self.index, "active", self.active

  def inactivate(self):
    self.active = False
    #print self.index, "active", self.active

class DoonoonFrame( wx.Frame):
  def __init__(self, parent, id, title):
    wx.Frame.__init__(self, parent, id, title, wx.DefaultPosition, windowSize )
    self.statusbar = self.CreateStatusBar()
    self.app = wx.GetApp()
    #self.SetSize( ( 800, 600 ) )

    panel = wx.Panel( self, -1 )

    self.panel1 = CamView( panel, -1, wx.BitmapFromImage(wx.EmptyImage(320,240)), size=(IMAGE_WIDTH,IMAGE_HEIGHT) )
    self.panel2 = CamView( panel, -1, wx.BitmapFromImage(wx.EmptyImage(320,240)), size=(IMAGE_WIDTH,IMAGE_HEIGHT) )
    #self.panel1 = wx.StaticBitmap( panel, -1, wx.BitmapFromImage(wx.EmptyImage(640,480)), size=(640,480) )
    #self.panel2 = wx.StaticBitmap( panel, -1, wx.BitmapFromImage(wx.EmptyImage(640,480)), size=(640,480) )
    #self.panel1 = wx.Panel( panel, -1, size=(320,240) )
    #self.panel2 = wx.Panel( panel, -1, size=(320,240) )
    #self.panel1.SetBackgroundColour( "blue" )
    #self.panel2.SetBackgroundColour( "green" )
    self.showImageButton = wx.Button( panel, ID_SHOWIMAGE_BUTTON, "Start Show" )
    self.endShowButton = wx.Button( panel, ID_ENDSHOW_BUTTON, "End Show" )
    self.calibrateButton = wx.Button( panel, ID_CALIBRATE_BUTTON, "Calibrate" )
    self.calibrateButton2 = wx.Button( panel, ID_CALIBRATE2_BUTTON, "Cal imgfile" )
    self.moveLeftButton = wx.Button( panel, ID_MOVELEFT_BUTTON, "<" )
    self.moveRightButton = wx.Button( panel, ID_MOVERIGHT_BUTTON, ">" )
    self.toggleLaserButton = wx.Button( panel, ID_TOGGLELASER_BUTTON, "Laser" )
    self.scanAngle = wx.TextCtrl(panel, ID_SCANANGLE, '90')
    self.scanButton = wx.Button( panel, ID_SCAN_BUTTON, "Scan" )
    self.testButton = wx.Button( panel, ID_TEST_BUTTON, "RecMap" )
    self.test2Button = wx.Button( panel, ID_TEST2_BUTTON, "SingleCal" )
    self.clearButton = wx.Button( panel, ID_CLEAR_BUTTON, "Clear" )

    self.chkFromFile = wx.CheckBox( panel, ID_FROM_FILE_BUTTON, "From File" )
    self.chkRemap = wx.CheckBox( panel, ID_CHK_REMAP, "Remap" )
    self.findChessboardButton = wx.Button( panel, ID_FIND_CHESSBOARD_BUTTON, "Find Board" )
    self.calibrateCameraButton = wx.Button( panel, ID_CALIBRATE_CAMERA_BUTTON, "Calibrate" )
    self.stereoCalibrateButton = wx.Button( panel, ID_STEREO_CALIBRATE_BUTTON, "Stereo Calib" )
    self.stereoRectifyButton = wx.Button( panel, ID_STEREO_RECTIFY_BUTTON, "Stereo Rectify" )
    self.rectifyMapButton = wx.Button( panel, ID_RECTIFY_MAP_BUTTON, "Rectify Map" )
    #self.rectifyMapButton = wx.Button( panel, ID_RECTIFY_MAP_BUTTON, "Rectify Map" )

    
    #self.portButton = wx.Button( panel, ID_PORT_BUTTON, "Port" )
    self.Bind(wx.EVT_BUTTON, self.BeginShow, id=ID_SHOWIMAGE_BUTTON)
    self.Bind(wx.EVT_BUTTON, self.EndShow, id=ID_ENDSHOW_BUTTON)
    self.Bind(wx.EVT_BUTTON, self.BeginCalibrate, id=ID_CALIBRATE_BUTTON)
    self.Bind(wx.EVT_BUTTON, self.BeginCalibrate2, id=ID_CALIBRATE2_BUTTON)
    self.Bind(wx.EVT_BUTTON, self.Test, id=ID_TEST_BUTTON)
    self.Bind(wx.EVT_BUTTON, self.Test2, id=ID_TEST2_BUTTON)
    self.Bind(wx.EVT_BUTTON, self.MoveLeft, id=ID_MOVELEFT_BUTTON)
    self.Bind(wx.EVT_BUTTON, self.MoveRight, id=ID_MOVERIGHT_BUTTON)
    self.Bind(wx.EVT_BUTTON, self.ToggleLaser, id=ID_TOGGLELASER_BUTTON)
    self.Bind(wx.EVT_BUTTON, self.Scan, id=ID_SCAN_BUTTON)
    self.Bind(wx.EVT_BUTTON, self.Clear, id=ID_CLEAR_BUTTON)

    self.Bind( wx.EVT_CHECKBOX, self.ToggleRemap, id=ID_CHK_REMAP)
    self.Bind(wx.EVT_BUTTON, self._FindChessBoard, id=ID_FIND_CHESSBOARD_BUTTON)
    self.Bind(wx.EVT_BUTTON, self._CalibrateCamera, id=ID_CALIBRATE_CAMERA_BUTTON)
    self.Bind(wx.EVT_BUTTON, self._StereoCalibrate, id=ID_STEREO_CALIBRATE_BUTTON)
    self.Bind(wx.EVT_BUTTON, self._StereoRectify, id=ID_STEREO_RECTIFY_BUTTON)
    self.Bind(wx.EVT_BUTTON, self._RectifyMap, id=ID_RECTIFY_MAP_BUTTON)
    #self.Bind(wx.EVT_BUTTON, self.ChangePort, id=ID_PORT_BUTTON)

    portlist = self.CheckSerialPort()
    self.serialCombo = wx.ComboBox( panel, ID_SERIAL_COMBO, "COM7", (15,30), wx.DefaultSize, portlist, wx.CB_DROPDOWN )
    self.Bind( wx.EVT_COMBOBOX, self.OnSerialCombo, id=ID_SERIAL_COMBO )
    #self.Bind(wx.EVT_BUTTON, self.ShowImage, id=ID_SHOWIMAGE_BUTTON)
    
    subSizer1 = wx.BoxSizer()
    #subSizer1.Add( self.chkShowIndex, flag=wx.EXPAND )
    subSizer1.Add( self.showImageButton, flag=wx.EXPAND )
    subSizer1.Add( self.endShowButton, flag=wx.EXPAND )
    subSizer1.Add( self.calibrateButton, flag=wx.EXPAND )
    subSizer1.Add( self.clearButton, flag=wx.EXPAND )
    
    scanSizer = wx.BoxSizer()
    scanSizer.Add( self.moveLeftButton, flag=wx.EXPAND )
    scanSizer.Add( self.moveRightButton, flag=wx.EXPAND )
    scanSizer.Add( self.toggleLaserButton, flag=wx.EXPAND )
    scanSizer.Add( self.serialCombo, flag=wx.EXPAND )
    scanSizer.Add( self.scanAngle, flag=wx.EXPAND )
    scanSizer.Add( self.scanButton, flag=wx.EXPAND )

    subSizer2 = wx.BoxSizer()
    subSizer2.Add( self.calibrateButton2, flag=wx.EXPAND )
    subSizer2.Add( self.testButton, flag=wx.EXPAND )
    subSizer2.Add( self.test2Button, flag=wx.EXPAND )

    subSizer3 = wx.BoxSizer()
    subSizer3.Add( self.chkFromFile, flag=wx.EXPAND )
    subSizer3.Add( self.chkRemap, flag=wx.EXPAND )
    subSizer3.Add( self.findChessboardButton, flag=wx.EXPAND )
    subSizer3.Add( self.calibrateCameraButton, flag=wx.EXPAND )
    subSizer3.Add( self.stereoCalibrateButton, flag=wx.EXPAND )
    subSizer3.Add( self.stereoRectifyButton, flag=wx.EXPAND )
    subSizer3.Add( self.rectifyMapButton, flag=wx.EXPAND )

    subSizer4 = wx.BoxSizer()
    subSizer5 = wx.BoxSizer()

    mainSizer = wx.GridBagSizer( hgap=5, vgap=5 )
    mainSizer.Add( self.panel1, pos=(0,0), flag=wx.EXPAND )
    mainSizer.Add( subSizer1, pos=(2,0), flag=wx.EXPAND )
    mainSizer.Add( scanSizer, pos=(2,2), flag=wx.EXPAND )
    mainSizer.Add( self.panel2, pos=(0,2), flag=wx.EXPAND )
    #mainSizer.Add( subSizer2, pos=(2,2), flag=wx.EXPAND )
    
    
    panel.SetSizer(mainSizer)
    panel.Fit()
    
    #print cvVersion
    wx.BeginBusyCursor()
    self.statusbar.SetStatusText( "Checking available camera(s)..." )

    self.camera = []
    self.max_devnum = self.CheckCamera()
    default_camera = [ 0, 1 ]
    
    arr = []
    for i in range(self.max_devnum):
      arr.append( str(i) )

    self.camTestCombo = wx.ComboBox( panel, ID_CAMTEST_COMBO, str(default_camera[0]), (15,30), wx.DefaultSize, arr, wx.CB_DROPDOWN )
    subSizer3.Add( self.camTestCombo, flag=wx.EXPAND )
    mainSizer.Add( subSizer3, pos=(3,0), flag=wx.EXPAND )
    
    self.statusbar.SetStatusText( "Adding camera(s)..." )
    self.cam1Combo = wx.ComboBox( panel, ID_CAM1_COMBO, str(default_camera[0]), (15,30), wx.DefaultSize, arr, wx.CB_DROPDOWN )
    self.Bind( wx.EVT_COMBOBOX, self.OnCam1Combo, id=ID_CAM1_COMBO )
    self.chkUseCamera1 = wx.CheckBox( panel, ID_CHK_USE_CAMERA1, "Use" )
    self.Bind( wx.EVT_CHECKBOX, self.UseCamera1, id=ID_CHK_USE_CAMERA1)
    subSizer4.Add( self.cam1Combo, flag=wx.EXPAND )
    subSizer4.Add( self.chkUseCamera1, flag=wx.EXPAND )
    mainSizer.Add( subSizer4, pos=(1,0), flag=wx.EXPAND)

    self.camera.append( DoonoonCam( default_camera[0] ) )
    self.chkUseCamera1.SetValue( True )
    #self.OnCam1Combo(None)
    self.UseCamera1(None)

    if self.max_devnum > 1:
      self.cam2Combo = wx.ComboBox( panel, ID_CAM2_COMBO, str( default_camera[1] ), (15,30), wx.DefaultSize, arr, wx.CB_DROPDOWN )
      self.Bind( wx.EVT_COMBOBOX, self.OnCam2Combo, id=ID_CAM2_COMBO )
      self.chkUseCamera2 = wx.CheckBox( panel, ID_CHK_USE_CAMERA2, "Use" )
      self.Bind( wx.EVT_CHECKBOX, self.UseCamera2, id=ID_CHK_USE_CAMERA2)
      subSizer5.Add( self.cam2Combo, flag=wx.EXPAND )
      subSizer5.Add( self.chkUseCamera2, flag=wx.EXPAND )
      #self.setCam2( default_camera[1] )
      #self.camera.append( self.cam2 )
      mainSizer.Add( subSizer5, pos=(1,2), flag=wx.EXPAND)
      self.camera.append( DoonoonCam( default_camera[1] ) )
      self.chkUseCamera2.SetValue( True)
      #self.OnCam2Combo(None)
      self.UseCamera2(None)
    
    
    #for cam in self.camera:
    #  cam.prepare_calibrate( )
    self.success_count = 0
    self.calibration_finished = False
    self.find_chessboard_finished = False
#    self.BeginShow(None)
    self.is_laser_on = False
    self.statusbar.SetStatusText( "Cameras are ready..." )
    self.is_serial_open = False
    self.is_calibrating = False
    wx.EndBusyCursor()
    
    
    self.serial = serial.Serial()
    self.ChangePort(None)
    #self.serial.port = "COM7"
    #res = self.serial.open()
    #print res
    #self.init_matrices( CHESSBOARD_WIDTH, CHESSBOARD_HEIGHT, N_CHESSBOARD )

    #self.cam1 = Device( 1 )
    #self.cam2 = Device( 2 )  
  def OnSerialCombo(self,evt):
    self.ChangePort(evt)
  def ChangePort(self,evt):
    #print self.serial.portstr
    if self.serial.isOpen():
      #print "closing"
      self.serial.close()
    self.serial.port = self.serialCombo.GetValue()
    self.serial.open()
    #print self.serial.portstr
    #print self.serial.isOpen()
  def UseCamera1(self,evt):
    self.use_camera1 = self.chkUseCamera1.GetValue()
    if self.use_camera1:
      self.camera[0].activate()
    else:
      self.camera[0].inactivate()
    return

  def Clear(self,evt):
    for cam in self.camera:
      cam.calibration_finished = False
      cam.rectification_finished = False
      cam.success_count = 0
    self.success_count= 0
    self.find_chessboard_finished = False
    
  def UseCamera2(self,evt):
    self.use_camera2 = self.chkUseCamera2.GetValue()
    if len( self.camera ) > 1 :
      if self.use_camera2:
        self.camera[1].activate()
      else:
        self.camera[1].inactivate()
    return

  def Scan(self,evt):
    #print "scan begin"
    wx.BeginBusyCursor()
    if not self.is_laser_on:
      self.ToggleLaser(None)
    angle = int(self.scanAngle.GetValue())
    step = int( angle / 0.9 )
    #print step, "steps"
    for i in range(step):
      self.SendToSerial( ">" )
      for cam in self.camera:
        cam.grab_image()
      for cam in self.camera:
        cam.save_image( "images/scan_" + str( cam.index ) + "_" + str( i + 1) + ".jpg")
      time.sleep(1)
    self.ToggleLaser(None)
    for i in range(step):
      self.SendToSerial("<")
    wx.EndBusyCursor()

  def MonitorSerial(self,evt):
    #self.serial.read
    pass

  def SendToSerial(self,msg):
    #print "send to serial"
    #ser = serial.Serial(self.serialPort.GetValue(), 9600, timeout=0)
    #self.serial.open("COM7")
    #ser.port = "COM7"
    #ser.open()
    #print ser.portstr
    self.serial.write(msg)
    #print msg
    buf = self.serial.inWaiting()
    #print buf
    msg = ''
    while( buf > 0 ):
      msg += self.serial.read()
      buf = self.serial.inWaiting()
    #print buf
    #print msg
    #ser.close()
    #self.serial.close()

  def CheckSerialPort(self):
    portlist = []
    for i in range(1,9):
      portstr = "COM" + str(i)
      try:
        ser = serial.Serial( portstr )
        portlist.append(portstr)
      except:
        pass
    return portlist

  def ToggleLaser(self,evt):
    #print "toggle laser"
    if not self.serial.isOpen():
      self.ChangePort(None)
    if self.is_laser_on:
      self.is_laser_on = False
      self.SendToSerial("0")
    else:
      self.is_laser_on = True
      self.SendToSerial("1")

  def InitSerial(self):
    pass

  def MoveRight(self,evt):
    #print "move right"
    self.SendToSerial(">")
  def MoveLeft(self,evt):
    #print "move left"
    self.SendToSerial("<")
  
  def OnCam1Combo(self,evt):
    #print "cam1combo"
    idx = self.cam1Combo.GetSelection()
    if len( self.camera ) > 1 :
      if idx == self.camera[1].index :
        wx.MessageBox( "Already in use!" )
        self.cam1Combo.SetSelection( self.cam1idx )
        return
    self.camera[0].set_camera( idx )

  def OnCam2Combo(self,evt):
    #print "cam2combo"
    if self.max_devnum <= 1:
      return
    idx = self.cam2Combo.GetSelection()
    if idx == self.camera[0].index:
      wx.MessageBox( "Already in use!" )
      self.cam2Combo.SetSelection( self.cam2idx )
      return
    self.camera[1].set_camera( idx )

  def CheckCamera(self):
    max_devnum = 0
    for i in range(10):
      try:
        #print "i:", i
        cam = Device(devnum=i)
      except ValueError, e:
        #print "error", e
        max_devnum = i
        #del cam
        break
    return max_devnum

  def BeginShow(self,evt):
    self.statusbar.SetStatusText( "Starting up the camera(s)..." )
    wx.BeginBusyCursor()
    for cam in self.camera:
      print "Camera", cam.index, "starting..."
      cam.grab_image()
    self.timer = wx.Timer(self)
    self.Bind(wx.EVT_TIMER, self.ShowImage, self.timer)
    self.timer.Start(100)
    wx.EndBusyCursor()
    self.statusbar.SetStatusText( "Camera started." )

  def EndShow(self,evt):
    self.Unbind(wx.EVT_TIMER, self.timer )

  def SetBitmap( self, pilimg, img_idx, size=(IMAGE_WIDTH,IMAGE_HEIGHT) ):
    #print "piltoimage", clock()
    wximg = piltoimage( pilimg, False)
    #print "piltoimage ends", clock()
    if img_idx == 1 :
      self.panel2.SetBitmap( wx.BitmapFromImage( wximg ) )
    else:
      self.panel1.SetBitmap( wx.BitmapFromImage( wximg ) )
    
  def ShowImage(self,evt):
    #cam1.setResolution( 320, 240 )
    #cam2.setResolution( 320, 240 )
    #begin_time = clock()
    images = []
    #print "begin show", clock()
    for cam in self.camera:
      if cam.active:
        cam.grab_image()
    for cam in self.camera:
      if cam.active:
        images.append( [ cam.index, cam.get_processed_image() ] )
      #indices.append( cam.index )
      #images.append( cam.get_image() )
    #print "after getimages", clock()
    #i = 0
    for img in images:
      self.SetBitmap( img[1], img[0] )
      #i += 1
    #print "after set bitmap", clock()
    #end_time = clock()
    #print "elapsed time:", end_time - begin_time
    return

  def init_matrices( self, board_w, board_h, n_boards ):
    self.board_w = board_w
    self.board_h = board_h
    self.board_n = board_w * board_h
    self.n_boards = n_boards
    self.intrinsic_matrix  = cvCreateMat( 3, 3, CV_32FC1 )
    self.distortion_coeffs = cvCreateMat( 4, 1, CV_32FC1 )
    self.point_counts      = cvCreateMat( self.n_boards, 1, CV_32SC1 )
    self.image_points      = cvCreateMat( self.n_boards * self.board_n, 2, CV_32FC1 )
    self.object_points     = cvCreateMat( self.n_boards * self.board_n, 3, CV_32FC1 )
    self.success_count = 0
    self.calibration_finished = False

  def init_secondarymatrices(self):
    self.point_counts2  = cvCreateMat( self.successes, 1, CV_32SC1 )
    self.image_points2  = cvCreateMat( self.successes * self.board_n, 2, CV_32FC1 )
    self.object_points2 = cvCreateMat( self.successes * self.board_n, 3, CV_32FC1 )

  def BeginCalibrate(self,evt):
    self.calibrate_from_file = False
    self.timer2 = wx.Timer(self)
    self.Bind(wx.EVT_TIMER, self.FindChessBoard, self.timer2)
    self.timer2.Start(1000)

  def BeginCalibrate2(self,evt):
    self.calibrate_from_file = True
    self.timer2 = wx.Timer(self)
    self.Bind(wx.EVT_TIMER, self.FindChessBoard, self.timer2)
    self.timer2.Start(1000)
    #print "from file:", self.calibrate_from_file 

  def FindChessBoard(self, evt):
    
    #print "success_count:", self.success_count
    #print "calibration_finished:", self.calibration_finished
    #print "calibrate_from_file", self.calibrate_from_file

    if( self.success_count == 0 ):
      for cam in self.camera:
        cam.begin_find_chessboard()

    #print self.calibrate_from_file
    if( self.success_count < N_CHESSBOARD):
      for cam in self.camera:
        if self.calibrate_from_file:
          cam.load_image( "images/calibrate_" + str( cam.index ) + "_" + str( cam.success_count + 1 ) + ".jpg" )
        else:
          cam.grab_image()
      success = 0
      total_success = 0
      for cam in self.camera:
        success = cam.find_chessboard()
        self.SetBitmap( cam.corner_img, cam.index )
        if success > 0 and not self.calibrate_from_file: 
          cam.save_image( "images/calibrate_" + str( cam.index ) + "_" + str( cam.success_count + 1 ) + ".jpg" )
        #print "cam", cam.index, "success", success, "total success", total_success
        total_success += success
      if total_success == 2:
        for cam in self.camera:
          cam.success_count += 1
        self.success_count += 1
        self.statusbar.SetStatusText("success: "+str(self.success_count))
      if self.success_count == N_CHESSBOARD:
        for cam in self.camera:
          cam.end_find_chessboard()
      #cvSave( "object_pointS.xml", self.object_points )
      return
    elif( self.find_chessboard_finished == False ):
      for cam in self.camera:
        cam.end_find_chessboard()
        #cam.save_matrices()
        cam.save_object_points()
        cam.save_image_points()
        cam.save_point_counts()
      self.Unbind(wx.EVT_TIMER, self.timer2 )
      self.find_chessboard_finished = True
    #  self.StereoCalibrate()

  def StereoCalibrate( self ):
      wx.BeginBusyCursor()
      N = CHESSBOARD_WIDTH * CHESSBOARD_HEIGHT * N_CHESSBOARD
      #object_points = cvMat( 1, N, CV_32FC3, self.camera[0].object_points )
      #image_points1 = cvMat( 1, N, CV_32FC2, self.camera[0].image_points )
      #image_points2 = cvMat( 1, N, CV_32FC2, self.camera[1].image_points )
      object_points = self.camera[0].object_points 
      image_points1 = self.camera[0].image_points
      image_points2 = self.camera[1].image_points
      M1 = self.camera[0].intrinsic_matrix
      M2 = self.camera[1].intrinsic_matrix
      cvSetIdentity( M1 )
      cvSetIdentity( M2 )
      D1 = self.camera[0].distortion_coeffs
      D2 = self.camera[1].distortion_coeffs
      D1 = cvCreateMat( 1, 5, CV_64F )
      D2 = cvCreateMat( 1, 5, CV_64F )
      cvZero( D1 )
      cvZero( D2 )
      matR = cvCreateMat( 3, 3, CV_64F )
      matT = cvCreateMat( 3, 1, CV_64F )
      matE = cvCreateMat( 3, 3, CV_64F )
      matF = cvCreateMat( 3, 3, CV_64F )
      cvZero( matR )
      cvZero( matT )
      cvZero( matE )
      cvZero( matF )
      cvSave( "matrices/object_points.xml", byref( object_points ) )
      cvSave( "matrices/image_points1.xml", byref( image_points1 ) )
      cvSave( "matrices/image_points2.xml", byref( image_points2 ) )
      self.StatusBar.SetStatusText("find chessboard finished. now calibrating")
      cvSave( "matrices/M1_1.xml", byref( M1 ) )
      cvSave( "matrices/M2_1.xml", byref( M2 ) )
      cvSave( "matrices/D1_1.xml", byref( D1 ) )
      cvSave( "matrices/D2_1.xml", byref( D2 ) )
      cvSave( "matrices/matR_1.xml", byref( matR ) )
      cvSave( "matrices/matT_1.xml", byref( matT ) )
      cvSave( "matrices/matE_1.xml", byref( matE ) )
      cvSave( "matrices/matF_1.xml", byref( matF ) )
      #( matR, matT, matE, matF ) = 
      ( M1, D1, M2, D2 ) = cvStereoCalibrate( object_points, image_points1, image_points2,
                         self.camera[0].point_counts, 
                         M1, D1,
                         M2, D2,
                         cvIMAGE_SIZE, matR, matT, matE, matF, cvTermCriteria( CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 100, 1e-5), CV_CALIB_FIX_ASPECT_RATIO + CV_CALIB_ZERO_TANGENT_DIST + CV_CALIB_SAME_FOCAL_LENGTH ) 
      cvSave( "matrices/M1_2.xml", byref( M1 ) )
      cvSave( "matrices/M2_2.xml", byref( M2 ) )
      cvSave( "matrices/D1_2.xml", byref( D1 ) )
      cvSave( "matrices/D2_2.xml", byref( D2 ) )
      cvSave( "matrices/matR_2.xml", byref( matR ) )
      cvSave( "matrices/matT_2.xml", byref( matT ) )
      cvSave( "matrices/matE_2.xml", byref( matE ) )
      cvSave( "matrices/matF_2.xml", byref( matF ) )
      self.StatusBar.SetStatusText("calibration done. begin rectification")
      ''' Do quality check later '''
      # cvUndistortPoints( self.camera[0].image_points, self.camera[1].image_points)
      wx.EndBusyCursor()
      #wx.MessageBox("rectification begin")
      #return
      '''rectification'''
      R1 = cvCreateMat( 3, 3, CV_64F )
      R2 = cvCreateMat( 3, 3, CV_64F )
      P1 = cvCreateMat( 3, 4, CV_64F )
      P2 = cvCreateMat( 3, 4, CV_64F )
      cvZero( R1 )
      cvZero( R2 )
      cvZero( P1 )
      cvZero( P2 )
      ( R1, R2, P1, P2 ) = cvStereoRectify( M1, M2, D1, D2, cvIMAGE_SIZE, matR, matT, R1, R2, P1, P2, None, 0 )
      #wx.MessageBox( "stereo rectify done.. now generating rectify map" )
      cvSave( "matrices/R1.xml", byref( R1 ) )
      cvSave( "matrices/R2.xml", byref( R2 ) )
      cvSave( "matrices/P1.xml", byref( P1 ) )
      cvSave( "matrices/P2.xml", byref( P2 ) )

      mx1 = cvCreateMat( IMAGE_WIDTH, IMAGE_HEIGHT, CV_32F )
      my1 = cvCreateMat( IMAGE_WIDTH, IMAGE_HEIGHT, CV_32F )
      mx2 = cvCreateMat( IMAGE_WIDTH, IMAGE_HEIGHT, CV_32F )
      my2 = cvCreateMat( IMAGE_WIDTH, IMAGE_HEIGHT, CV_32F )
      img1r = cvCreateMat( IMAGE_WIDTH, IMAGE_HEIGHT, CV_8U )
      img2r = cvCreateMat( IMAGE_WIDTH, IMAGE_HEIGHT, CV_8U )
      disp = cvCreateMat( IMAGE_WIDTH, IMAGE_HEIGHT, CV_16S )
      vdisp = cvCreateMat( IMAGE_WIDTH, IMAGE_HEIGHT, CV_8U )
      cvZero( mx1 )
      cvZero( my1 )
      cvZero( mx2 )
      cvZero( my2 )
      #( M1_new, mx1, my1 ) = cvInitUndistortRectifyMap( M1, D1, R1, None, mx1, my1 )
      #( M2_new, mx2, my2 ) = cvInitUndistortRectifyMap( M2, D2, R2, None, mx2, my2 )
      #wx.MessageBox( "rectify map obtained. assigning maps to cameras" )
      #cvSave( "matrices/M1_new.xml", byref( M1_new ) )
      #cvSave( "matrices/M2_new.xml", byref( M2_new ) )
      cvSave( "matrices/mx1.xml", byref( mx1 ) )
      cvSave( "matrices/my1.xml", byref( my1) )
      cvSave( "matrices/mx2.xml", byref( mx2 ) )
      cvSave( "matrices/my2.xml", byref( my2 ) )
      #self.camera[0].calibrate( intrinsic = M1, distortion = D1 )
      #self.camera[1].calibrate( intrinsic = M2, distortion = D2 )
      self.camera[0].rectmapx = mx1
      self.camera[0].rectmapy = my1
      self.camera[1].rectmapx = mx2
      self.camera[1].rectmapy = my2
      self.camera[0].rectification_finished = True
      self.camera[1].rectification_finished = True
      self.StatusBar.SetStatusText("calibration done. begin rectification")
      
      #print matR, matT, matE, matF
  def GetNextImage(self):
    img1 = self.cam1.getImage()
    #self.SetBitmap( img1, 1 )

    filename = "images/cam" + str( self.successes ) + ".jpg"
    img1.save(  filename )
    return cvLoadImage( filename)

  def Test(self,evt):
    M1 = cvLoadCast( "matrices/M1_2.xml", CvMat )
    D1 = cvLoadCast( "matrices/D1_2.xml", CvMat )
    M2 = cvLoadCast( "matrices/M2_2.xml", CvMat )
    D2 = cvLoadCast( "matrices/D2_2.xml", CvMat )

    M1 = cvLoadCast( "matrices/intrinsics_0.xml", CvMat )
    M2 = cvLoadCast( "matrices/intrinsics_1.xml", CvMat )
    D1 = cvLoadCast( "matrices/distortion_0.xml", CvMat )
    D2 = cvLoadCast( "matrices/distortion_1.xml", CvMat )

    R1 = cvLoadCast( "matrices/R1.xml", CvMat )
    R2 = cvLoadCast( "matrices/R2.xml", CvMat )
    P1 = cvLoadCast( "matrices/P1.xml", CvMat )
    P2 = cvLoadCast( "matrices/P2.xml", CvMat )
    print M1
    print D1
    print R1
    mx1 = cvCreateMat( IMAGE_WIDTH, IMAGE_HEIGHT, CV_32F )
    my1 = cvCreateMat( IMAGE_WIDTH, IMAGE_HEIGHT, CV_32F )
    mx1 = cvCreateImage( cvIMAGE_SIZE, IPL_DEPTH_32F, 1 )
    my1 = cvCreateImage( cvIMAGE_SIZE, IPL_DEPTH_32F, 1 )
    mx2 = cvCreateMat( IMAGE_WIDTH, IMAGE_HEIGHT, CV_32F )
    my2 = cvCreateMat( IMAGE_WIDTH, IMAGE_HEIGHT, CV_32F )
    mx2 = cvCreateImage( cvIMAGE_SIZE, IPL_DEPTH_32F, 1 )
    my2 = cvCreateImage( cvIMAGE_SIZE, IPL_DEPTH_32F, 1 )
    cvSave( "matrices/mx1_1.xml", byref( mx1 ) ) 
    cvSave( "matrices/my1_1.xml", byref( my1 ) ) 
    cvSave( "matrices/mx2_1.xml", byref( mx2 ) ) 
    cvSave( "matrices/my2_1.xml", byref( my2 ) )
    #print M1
    #( M1_new, mx1_new, my1_new ) = cvInitUndistortRectifyMap( M1, D1, R1, None, mx1, my1 )
    #( M2_new, mx2_new, my2_new ) = cvInitUndistortRectifyMap( M2, D2, R2, None, mx2, my2 )
    print "begin undistort rectify map", clock()
    ( M1_new, mx1_new, my1_new ) = _cvInitUndistortRectifyMap( M1, D1, R1, P1, mx1, my1 )
    ( M1_new, mx2_new, my2_new ) = _cvInitUndistortRectifyMap( M2, D2, R2, P2, mx2, my2 )
    print "end undistort rectify map", clock()
    print M1
    print D1
    print R1
    #cvSave( "matrices/M1_new.xml", byref( M1_new ) ) 
    #cvSave( "matrices/M2_new.xml", byref( M2_new ) ) 
    cvSave( "matrices/mx1_2.xml", byref( mx1_new ) ) 
    cvSave( "matrices/my1_2.xml", byref( my1_new ) ) 
    cvSave( "matrices/mx2_2.xml", byref( mx2_new ) ) 
    cvSave( "matrices/my2_2.xml", byref( my2_new ) )
    self.camera[0].rectmapx = mx1
    self.camera[0].rectmapy = my1
    self.camera[1].rectmapx = mx2
    self.camera[1].rectmapy = my2
    self.camera[0].rectification_finished = True
    self.camera[1].rectification_finished = True
    img = Image.open( "images/rect_0.jpg" )
    ipl_img = pil_to_ipl( img )
    t = cvCloneImage( ipl_img )
    print t
    print ipl_img
    print mx1
    print my1
    print "remap begin", clock()
    cvRemap( t, ipl_img, mx1, my1 )
    print "remap end", clock()
    img = ipl_to_pil( ipl_img )
    img.save( "images/rect_remapped_0.jpg" )

  def Test2(self,evt):
    self.calibrate_from_file = True
    for i in range(10):
      self.FindChessBoard(None)
      time.sleep(1)
    for cam in self.camera:
      cam.calibrate()
      cam.calibration_finished = True

  def ToggleRemap(self,evt):
    value = self.chkRemap.GetValue()
    for cam in self.camera:
      cam.remap = value

  def _FindChessBoard(self,evt):
    self.calibrate_from_file = self.chkFromFile.GetValue()
    #print "from file", self.calibrate_from_file
    self.timer2 = wx.Timer(self)
    self.Bind(wx.EVT_TIMER, self.FindChessBoard, self.timer2)
    self.timer2.Start(1000)

  def _CalibrateCamera(self,evt):
    if self.chkFromFile.GetValue():
      for cam in self.camera:
        cam.load_object_points()
        cam.load_image_points()
        cam.load_point_counts()
      
    for cam in self.camera:
      cam.calibrate()
      cam.save_intrinsics()
      cam.save_distcoeffs()
      cam.save_maps()

  def _StereoCalibrate(self,evt):
    if len( self.camera ) < 2:
      wx.MessageBox( "Not enough camera.")
      return
    wx.BeginBusyCursor()
    
    if self.chkFromFile.GetValue():
      for cam in self.camera:
        cam.load_object_points()
        cam.load_image_points()
        cam.load_point_counts()
        cam.load_intrinsics()
        cam.load_distcoeffs()

    object_points = self.camera[0].object_points 
    image_points1 = self.camera[0].image_points
    image_points2 = self.camera[1].image_points
    M1 = self.camera[0].intrinsic_matrix
    M2 = self.camera[1].intrinsic_matrix
    D1 = self.camera[0].distortion_coeffs
    D2 = self.camera[1].distortion_coeffs

    matR = cvCreateMat( 3, 3, CV_64F )
    matT = cvCreateMat( 3, 1, CV_64F )
    matE = cvCreateMat( 3, 3, CV_64F )
    matF = cvCreateMat( 3, 3, CV_64F )
    cvZero( matR )
    cvZero( matT )
    cvZero( matE )
    cvZero( matF )
    self.StatusBar.SetStatusText("find chessboard finished. now calibrating")
    #( matR, matT, matE, matF ) = 
    ( M1, D1, M2, D2 ) = cvStereoCalibrate( object_points, image_points1, image_points2,
                       self.camera[0].point_counts, 
                       M1, D1,
                       M2, D2,
                       cvIMAGE_SIZE, matR, matT, matE, matF, cvTermCriteria( CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 100, 1e-5), CV_CALIB_FIX_ASPECT_RATIO + CV_CALIB_ZERO_TANGENT_DIST + CV_CALIB_SAME_FOCAL_LENGTH ) 
    cvSave( "matrices/M1.xml", byref( M1 ) )
    cvSave( "matrices/M2.xml", byref( M2 ) )
    cvSave( "matrices/D1.xml", byref( D1 ) )
    cvSave( "matrices/D2.xml", byref( D2 ) )
    cvSave( "matrices/matR.xml", byref( matR ) )
    cvSave( "matrices/matT.xml", byref( matT ) )
    cvSave( "matrices/matE.xml", byref( matE ) )
    cvSave( "matrices/matF.xml", byref( matF ) )
    self.StatusBar.SetStatusText("calibration done.")
    wx.EndBusyCursor()

  def _StereoRectify(self,evt):
    if len( self.camera ) < 2:
      wx.MessageBox( "Not enough camera.")
      return

    wx.BeginBusyCursor()
    if self.chkFromFile.GetValue():
      for cam in self.camera:
        cam.load_object_points()
        cam.load_image_points()
        cam.load_point_counts()
        cam.load_intrinsics()
        cam.load_distcoeffs()
        
    M1 = self.camera[0].intrinsic_matrix
    M2 = self.camera[1].intrinsic_matrix
    D1 = self.camera[0].distortion_coeffs
    D2 = self.camera[1].distortion_coeffs
      
    '''rectification'''
    R1 = cvCreateMat( 3, 3, CV_64F )
    R2 = cvCreateMat( 3, 3, CV_64F )
    P1 = cvCreateMat( 3, 4, CV_64F )
    P2 = cvCreateMat( 3, 4, CV_64F )
    cvZero( R1 )
    cvZero( R2 )
    cvZero( P1 )
    cvZero( P2 )

    matR = cvLoadCast( "matrices/matR.xml", CvMat )
    matT = cvLoadCast( "matrices/matT.xml", CvMat )

    ( R1, R2, P1, P2 ) = cvStereoRectify( M1, M2, D1, D2, cvIMAGE_SIZE, matR, matT, R1, R2, P1, P2, None, 0 )
    #wx.MessageBox( "stereo rectify done.. now generating rectify map" )
    cvSave( "matrices/R1.xml", byref( R1 ) )
    cvSave( "matrices/R2.xml", byref( R2 ) )
    cvSave( "matrices/P1.xml", byref( P1 ) )
    cvSave( "matrices/P2.xml", byref( P2 ) )
    wx.EndBusyCursor()


  def _RectifyMap(self,evt):
    if len( self.camera ) < 2:
      wx.MessageBox( "Not enough camera.")
      return
    wx.BeginBusyCursor()
    
    if self.chkFromFile.GetValue():
      for cam in self.camera:
        cam.load_object_points()
        cam.load_image_points()
        cam.load_point_counts()
        cam.load_intrinsics()
        cam.load_distcoeffs()
        
    M1 = self.camera[0].intrinsic_matrix
    M2 = self.camera[1].intrinsic_matrix
    D1 = self.camera[0].distortion_coeffs
    D2 = self.camera[1].distortion_coeffs

    R1 = cvLoadCast( "matrices/R1.xml", CvMat )
    R2 = cvLoadCast( "matrices/R2.xml", CvMat )
    P1 = cvLoadCast( "matrices/P1.xml", CvMat )
    P2 = cvLoadCast( "matrices/P2.xml", CvMat )

    for cam in self.camera:
      cam.rectmapx = cvCreateImage( cvIMAGE_SIZE, IPL_DEPTH_32F, 1 )
      cam.rectmapy = cvCreateImage( cvIMAGE_SIZE, IPL_DEPTH_32F, 1 )

    for cam in self.camera:
      ( z, mx_new, my_new ) = cvInitUndistortRectifyMap( cam.intrinsic_matrix, cam.distortion_coeffs, R1, P1, cam.rectmapx, cam.rectmapy )
      #cvSave( "matrices/rectmapx" + str( cam.index ) + ".xml", byref( mx_new ) ) 
      #cvSave( "matrices/rectmapy" + str( cam.index ) + ".xml", byref( my_new ) ) 
      cam.rectmapx = mx_new
      cam.rectmapy = my_new
      cam.save_rectmaps()
      cam.rectification_finished = True

    #self.camera[1].rectmapx = mx2
    #self.camera[1].rectmapy = my2
    wx.EndBusyCursor()

def piltoimage(pil,alpha=True):
    """Convert PIL Image to wx.Image."""
    if alpha:
        #print "alpha 1", clock()
        image = apply( wx.EmptyImage, pil.size )
        #print "alpha 2", clock()
        image.SetData( pil.convert( "RGB").tostring() )
        #print "alpha 3", clock()
        image.SetAlphaData(pil.convert("RGBA").tostring()[3::4])
        #print "alpha 4", clock()
    else:
        #print "no alpha 1", clock()
        image = wx.EmptyImage(pil.size[0], pil.size[1])
        #print "no alpha 2", clock()
        new_image = pil.convert('RGB')
        #print "no alpha 3", clock()
        data = new_image.tostring()
        #print "no alpha 4", clock()
        image.SetData(data)
        #print "no alpha 5", clock()
    return image

def imagetopil(image):
    """Convert wx.Image to PIL Image."""
    pil = Image.new('RGB', (image.GetWidth(), image.GetHeight()))
    pil.fromstring(image.GetData())
    return pil
    
    
def _cvInitUndistortRectifyMap( A, distCoeffs, R, Ar, mapx, mapy ):
  coi1 = 0
  coi2 = 0
  a = cvCreateMat( 3, 3, CV_64F )
  k = cvCreateMat( 4, 1, CV_64F )
  ar = cvCreateMat( 3, 3, CV_64F )
  r = cvCreateMat( 3, 3, CV_64F )
  ir = cvCreateMat( 3, 3, CV_64F )
  #k = distCoeffs

  for i in range(3):
    for j in range(3):
      a[i,j] = A[i,j]
      r[i,j] = R[i,j]
      ar[i,j] = Ar[i,j]
  cvSetIdentity( ar )
  cvZero( k )
       
  for i in range(4):
    #print i
    aa = distCoeffs[i,0]
    k[i,0] = aa
    
  cvMatMul( ar, r, r )
  cvInvert( r, ir )

  #for i in range(3):
  #  for j in range(3):
      #print r[i,j]

  #for i in range(3):
  #  for j in range(3):
      #print ir[i,j]

  u0 = a[0,2]
  v0 = a[1,2]
  fx = a[0,0]
  fy = a[1,1]
  k1 = k[0,0]
  k2 = k[1,0]
  k3 = 0
  p1 = k[2,0]
  p2 = k[3,0]
  
  size = cvIMAGE_SIZE
  for i in range( size.height ):
    _x = i * ir[0,1] + ir[0,2]
    _y = i * ir[1,1] + ir[1,2]
    _w = i * ir[2,1] + ir[2,2]
    for j in range( size.width ):
      _x += ir[0,0]
      _y += ir[1,0]
      _w += ir[2,0]
      w = 1.0 / _w
      x = _x * w 
      y = _y * w
      x2 = x*x
      y2 = y*y
      r2 = x2 + y2
      _2xy = 2*x*y
      kr = 1 + ( (k3*r2+k2)*r2+k1)*r2
      u = fx*(x*kr +p1*_2xy + p2*(r2+2*x2)) + u0
      v = fy*(y*kr +p1*(r2+2*y2) +p2*_2xy) + v0
      #print u
      #print v
      mapx[i,j] = u
      mapy[i,j] = v
  return ( mapx, mapy )

'''void
cvInitUndistortRectifyMap( const CvMat* A, const CvMat* distCoeffs,
    const CvMat *R, const CvMat* Ar, CvArr* mapxarr, CvArr* mapyarr )
{
    CV_FUNCNAME( "cvInitUndistortMap" );

    __BEGIN__;
    
    double a[9], ar[9], r[9], ir[9], k[5]={0,0,0,0,0};
    int coi1 = 0, coi2 = 0;
    CvMat mapxstub, *_mapx = (CvMat*)mapxarr;
    CvMat mapystub, *_mapy = (CvMat*)mapyarr;
    CvMat _a = cvMat( 3, 3, CV_64F, a );
    CvMat _k = cvMat( 4, 1, CV_64F, k );
    CvMat _ar = cvMat( 3, 3, CV_64F, ar );
    CvMat _r = cvMat( 3, 3, CV_64F, r );
    CvMat _ir = cvMat( 3, 3, CV_64F, ir );
    int i, j;
    double fx, fy, u0, v0, k1, k2, k3, p1, p2;
    CvSize size;

    CV_CALL( _mapx = cvGetMat( _mapx, &mapxstub, &coi1 ));
    CV_CALL( _mapy = cvGetMat( _mapy, &mapystub, &coi2 ));

    if( coi1 != 0 || coi2 != 0 )
        CV_ERROR( CV_BadCOI, "The function does not support COI" );

    if( CV_MAT_TYPE(_mapx->type) != CV_32FC1 )
        CV_ERROR( CV_StsUnsupportedFormat, "Both maps must have 32fC1 type" );

    if( !CV_ARE_TYPES_EQ( _mapx, _mapy ))
        CV_ERROR( CV_StsUnmatchedFormats, "" );

    if( !CV_ARE_SIZES_EQ( _mapx, _mapy ))
        CV_ERROR( CV_StsUnmatchedSizes, "" );

    if( A )
    {
        if( !CV_IS_MAT(A) || A->rows != 3 || A->cols != 3  ||
            (CV_MAT_TYPE(A->type) != CV_32FC1 && CV_MAT_TYPE(A->type) != CV_64FC1) )
            CV_ERROR( CV_StsBadArg, "Intrinsic matrix must be a valid 3x3 floating-point matrix" );
        cvConvert( A, &_a );
    }
    else
        cvSetIdentity( &_a );

    if( Ar )
    {
        CvMat Ar33;
        if( !CV_IS_MAT(Ar) || Ar->rows != 3 || (Ar->cols != 3 && Ar->cols != 4) ||
            (CV_MAT_TYPE(Ar->type) != CV_32FC1 && CV_MAT_TYPE(Ar->type) != CV_64FC1) )
            CV_ERROR( CV_StsBadArg, "The new intrinsic matrix must be a valid 3x3 floating-point matrix" );
        cvGetCols( Ar, &Ar33, 0, 3 );
        cvConvert( &Ar33, &_ar );
    }
    else
        cvSetIdentity( &_ar );

    if( !CV_IS_MAT(R) || R->rows != 3 || R->cols != 3  ||
        (CV_MAT_TYPE(R->type) != CV_32FC1 && CV_MAT_TYPE(R->type) != CV_64FC1) )
        CV_ERROR( CV_StsBadArg, "Rotaion/homography matrix must be a valid 3x3 floating-point matrix" );

    if( distCoeffs )
    {
        CV_ASSERT( CV_IS_MAT(distCoeffs) &&
            (distCoeffs->rows == 1 || distCoeffs->cols == 1) &&
            (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) == 4 ||
            distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) == 5) &&
            (CV_MAT_DEPTH(distCoeffs->type) == CV_64F ||
            CV_MAT_DEPTH(distCoeffs->type) == CV_32F) );
        _k = cvMat( distCoeffs->rows, distCoeffs->cols,
                CV_MAKETYPE(CV_64F, CV_MAT_CN(distCoeffs->type)), k );
        cvConvert( distCoeffs, &_k );
    }
    else
        cvZero( &_k );

    
    cvConvert( R, &_r );    // rectification matrix
    cvMatMul( &_ar, &_r, &_r ); // Ar*R
    cvInvert( &_r, &_ir );  // inverse: R^-1*Ar^-1

    u0 = a[2]; v0 = a[5];
    fx = a[0]; fy = a[4];
    k1 = k[0]; k2 = k[1]; k3 = k[4];
    p1 = k[2]; p2 = k[3];

    size = cvGetMatSize(_mapx);

    for( i = 0; i < size.height; i++ )
    {
        float* mapx = (float*)(_mapx->data.ptr + _mapx->step*i);
        float* mapy = (float*)(_mapy->data.ptr + _mapy->step*i);
        double _x = i*ir[1] + ir[2], _y = i*ir[4] + ir[5], _w = i*ir[7] + ir[8];

        for( j = 0; j < size.width; j++, _x += ir[0], _y += ir[3], _w += ir[6] )
        {
            double w = 1./_w, x = _x*w, y = _y*w;
            double x2 = x*x, y2 = y*y;
            double r2 = x2 + y2, _2xy = 2*x*y;
            double kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2;
            double u = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2)) + u0;
            double v = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy) + v0; 
            mapx[j] = (float)u;
            mapy[j] = (float)v;
        }
    }

    __END__;
}
'''